/*
 * uwb.c
 *
 *  Created on: 2022年5月18日
 *      Author: Cowlin
 */
#include <stdio.h>
#include <stdint.h>
#include <stddef.h>
#include <string.h>
#include <stdlib.h>
#include "esp_wifi.h"
#include "esp_system.h"
#include "nvs_flash.h"
#include "esp_event.h"
#include "esp_netif.h"

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "freertos/queue.h"
#include "esp_log.h"
#include "mqtt_client.h"
#include <unistd.h>

#include <hmi.h>
#include <rtk.h>

#include "dwm_api.h"
#include "uwb.h"
#include "esp_event_base.h"
static const char *UWB_TAG = "UWB";
#define CONFIG_UWB_RING_BUFFER_SIZE 1024
#define UWB_RUNTIME_BUFFER_SIZE (CONFIG_UWB_RING_BUFFER_SIZE/2)
#define UWB_EVENT_LOOP_QUEUE_SIZE (16)
#define CONFIG_UWB_TASK_STACK_SIZE 4096
#define CONFIG_UWB_TASK_PRIORITY 2



uwb_config_t config = UWB_CONFIG_DEFALT();

static void uwb_task_entry(void *arg) {
	esp_uwb_t *esp_uwb = (esp_uwb_t *)arg;
	dwm_status_t status;
	int rv;
	while (1) {

	rv = dwm_status_get(&status);
	if (rv == DWM_OK)
		{
			if (status.loc_data == 1)
			{
			dwm_pos_get(&(esp_uwb->parent.pos));
			esp_event_post_to(esp_uwb->event_loop_hdl,"ESP_UWB_EVENT",UWB_UPDATE,&(esp_uwb->parent),sizeof(uwb_t),100 / portTICK_PERIOD_MS);
			led_set(2, "0000ff",20);
			}else{
			dwm_pos_get(&(esp_uwb->parent.pos));
			esp_event_post_to(esp_uwb->event_loop_hdl,"ESP_UWB_EVENT",UWB_NOT_UPDATE,&(esp_uwb->parent),sizeof(uwb_t),100/portTICK_PERIOD_MS);
			}
		}
		esp_event_loop_run(esp_uwb->event_loop_hdl,pdMS_TO_TICKS(50));
	}
	vTaskDelete(NULL);
}

uwb_handle_t uwb_init(const uwb_config_t *config) {
	dwm_init();
	esp_uwb_t *esp_uwb = calloc(1, sizeof(esp_uwb_t));
	if (!esp_uwb) {
		ESP_LOGE(UWB_TAG, "calloc memory for esp_uwb failed");
		goto err_uwb;
	}
	esp_uwb->buffer = calloc(1,UWB_RUNTIME_BUFFER_SIZE);
	if(!esp_uwb->buffer){
		ESP_LOGE(UWB_TAG,"calloc memory for runtime buffer failed");
	}
	/* Create Event loop */
	esp_event_loop_args_t loop_args = {
			.queue_size = UWB_EVENT_LOOP_QUEUE_SIZE,
			.task_name = NULL
	};
	if(esp_event_loop_create(&loop_args,&esp_uwb->event_loop_hdl)!=ESP_OK){
		ESP_LOGE(UWB_TAG,"create event loop failed");
		goto err_create_loop;
	}
	BaseType_t err = xTaskCreate(
						uwb_task_entry,
						"uwb_location_get",
						CONFIG_UWB_TASK_STACK_SIZE,
						esp_uwb,
						CONFIG_UWB_TASK_PRIORITY,
						&esp_uwb->uwb_tsk_hdl
	);
	if(err!=pdTRUE){
		ESP_LOGE(UWB_TAG,"create UWB TAG task failed");
		goto err_uwb_task_create;
	}
	return esp_uwb;
	err_create_loop:
	err_uwb: free(esp_uwb);
	err_uwb_task_create:vTaskDelete(esp_uwb->uwb_tsk_hdl);
	return NULL;

}
void uwb_deinit(uwb_handle_t uwb_hdl)
{
	esp_uwb_t *esp_uwb = (esp_uwb_t *)uwb_hdl;
	vTaskDelete(esp_uwb->uwb_tsk_hdl);
	esp_event_loop_delete(esp_uwb->event_loop_hdl);
	dwm_deinit();
	free(esp_uwb->buffer);
	free(esp_uwb);
}

esp_err_t uwb_add_handler(uwb_handle_t uwb_hdl,esp_event_handler_t event_handler,void *handler_args)
{
	esp_uwb_t *esp_uwb = (esp_uwb_t *)uwb_hdl;
	return esp_event_handler_register_with(esp_uwb->event_loop_hdl,ESP_UWB_EVENT,ESP_EVENT_ANY_ID,event_handler,handler_args);

}
esp_err_t uwb_remove_handler(uwb_handle_t uwb_hdl , esp_event_handler_t event_handler)
{
	esp_uwb_t *esp_uwb = (esp_uwb_t *)uwb_hdl;
	return esp_event_handler_unregister_with(esp_uwb->event_loop_hdl,ESP_UWB_EVENT,ESP_EVENT_ANY_ID,event_handler);
}
